//Ping sensor

#include <avr/io.h>
#include <avr/interrupt.h>

void uart_write(unsigned char c)
{
	while(!(UCSR0A&(1<<UDRE0))); //wait until uart is ready
	UDR0=c; //send c out of uart
}

void uart_write_du16(unsigned int n) //Unsigned int as decimal printing
{
	unsigned char i=0, m=0;
	unsigned int k[5]={10000, 1000, 100, 10, 1};
	while(m<5)
	{
		i=0;
		while(n>(k[m]-1))
		{
			n-=k[m];
			i++;
		}
		uart_write('0'+i);
		m++;
	}
}

void uart_write_s(char *s)
{
	while(*s)
	{
		uart_write(*s);
		s++;
	}
}

void gpio_setup(void)
{
	DDRD|=(1<<PD7); //PD7 as output
	PORTD&=~(1<<PD7); //PD7 to ground
	DDRB&=~(1<<PB0); //PB0 as input
	PORTB&=~(1<<PB0); //PB0 no pullup
}

void uart_setup(void)
{
	UCSR0A=(1<<U2X0); //Double speed uart
	UCSR0B=(1<<TXEN0); //Enable tx circuitry.
	UCSR0C=(1<<UCSZ01)|(1<<UCSZ00); //Select 8-bit data
	UBRR0=1; //Select 1MBaud speed
}

void timer_setup(void)
{
	TCNT1=0; //Clear TIMER1 value
	OCR1A=9; //5us when TIMER1 is divided by 8
	TIMSK1=(1<<OCIE1A); //Enable compare match interrupt
	TCCR1B=(1<<ICES1)|(1<<CS11); //Trigger input capture on rising edge, divide TIMER1 by 8 and start TIMER1
}

ISR(TIMER1_COMPA_vect)
{
	if(PORTD&(1<<PD7)) //Test if query pulse on
	{
		PORTD&=~(1<<PD7); //End query pulse
		TIMSK1=(1<<ICIE1); //Disable compare match interrupt and enable input capture interrupt
	}
	else //Query pulse not started
	{
		PORTD|=(1<<PD7); //Start query pulse
	}
	TCNT1=0; //Clear TIMER1 value
}

ISR(TIMER1_CAPT_vect)
{
	TCCR1B&=~(1<<CS11); //Stop TIMER1
	if(PINB&(1<<PB0)) //Test if response pulse started
	{
		TCCR1B&=~(1<<ICES1); //Next timer trigger on falling edge
	}
	else //Response pulse ended
	{
		uart_write_s("Result: ");
		uart_write_du16(ICR1); //Print ICR1 value
		uart_write_s("\r\n");
		TIMSK1=(1<<OCIE1A); //Disable input capture interrupt and enable compare match interrupt
		TCCR1B|=(1<<ICES1); //Next timer trigger on rising edge
	}
	TCNT1=0; //Clear TIMER1 value
	TCCR1B|=(1<<CS11); //Start TIMER1
}

int main(void)
{
	gpio_setup(); //Configure gpio
	uart_setup(); //Configure uart
	timer_setup(); //Configure timer
	uart_write_s("Ready\r\n"); //Notify of start
	sei(); //Enable interrupts
	while(1); //Wait forever
	return(0); //Magic
}